
#include <limits.h>
#include <stddef.h>
#include <stdbool.h>
#include <stdint.h>

#include <log_util.h>
#include <memory_util.h>


static inline bool is_memaligned(void *m_ptr)
{
	return ((intptr_t)m_ptr & (intptr_t)((4 * sizeof(void*)) - 1));
}

void *e_malloc_safe(size_t size, const char *funcname, const char *filename, const int linenum)
{
	void *ptr; /* Pointer to new memory */

	if (!size)
	{
		log_output(log_level_warning, funcname, filename, linenum, "Memory allocation of 0 bytes requested");
		return NULL;
	}

	if (!(ptr = malloc(size)))
	{
		log_output(log_level_normal, funcname, filename, linenum, "Memory allocation of %lu bytes failed", (unsigned long) size);
		perror("malloc");
		exit(EXIT_FAILURE);
	}

	return ptr;
}

void *e_memalign_safe(size_t size, const char *funcname, const char *filename, const int linenum)
{
	void *ptr; /* Pointer to new memory */

	if (!size)
	{
		log_output(log_level_warning, funcname, filename, linenum, "Memory allocation of 0 bytes requested");
		return NULL;
	}

	if (posix_memalign( &ptr, 4 * sizeof(void*), size))
	{
		log_output(log_level_normal, funcname, filename, linenum, "Memory allocation of %lu bytes failed", (unsigned long) size);
		perror("malloc");
		exit(EXIT_FAILURE);
	}

	return ptr;
}

void *e_rememalign_safe(void *oldptr, size_t size, const char *funcname, const char *filename, const int linenum)
{
	void *ptr; /* Pointer to new memory */
	void *aligned_ptr;

	if (!(ptr = realloc(oldptr, size)))
	{
		log_output(log_level_normal, funcname, filename, linenum, "Memory reallocation of %zu bytes failed", size);
		perror("realloc");
		exit(EXIT_FAILURE);
	}

	/// If the pointer lost alignment, we need to re-allocate a new chunk
	if (! is_memaligned(ptr))
	{
		aligned_ptr = e_memalign_safe(size, funcname, filename, linenum);
		memcpy(aligned_ptr, ptr, size);
		free(ptr);
		ptr = aligned_ptr;
	}

	return ptr;
}

void *e_calloc_safe(size_t num, size_t size, const char *funcname, const char *filename, const int linenum)
{
	void *ptr; /* Pointer to new memory */

	if (size == 0 || num == 0)
	{
		log_output(log_level_warning, funcname, filename, linenum, "Memory allocation of 0 bytes requested");
		return NULL;
	}

	if (!(ptr = calloc(num, size)))
	{
		log_output(log_level_normal, funcname, filename, linenum, "Memory allocation of %lu bytes failed", (unsigned long) (size * size));
		perror("calloc");
		exit(EXIT_FAILURE);
	}

	return ptr;
}

void *e_realloc_safe(void *oldptr, size_t size, const char *funcname, const char *filename, const int linenum)
{
	void *ptr; /* Pointer to new memory */

	if (!(ptr = realloc(oldptr, size)))
	{
		log_output(log_level_normal, funcname, filename, linenum, "Memory reallocation of %lu bytes failed", (unsigned long) size);
		perror("realloc");
		exit(EXIT_FAILURE);
	}

	return ptr;
}

char *e_strdup_safe(const char *src, const char *funcname, const char *filename, const int linenum)
{
	void *dest; /* Pointer to new string */
	size_t len; /* String length */

	if (!src)
	{
		log_output(log_level_warning, funcname, filename, linenum, "strdup() of NULL string requested");
		return NULL;
	}

	len = strlen(src) + 1;

	if (!(dest = malloc(len * sizeof(char))))
	{
		log_output(log_level_normal, funcname, filename, linenum, "strdup of %lu bytes failed", (unsigned long) len);
		perror("strdup");
		exit(EXIT_FAILURE);
	}

	memcpy(dest, src, len * sizeof(char));

	return dest;
}

char *e_strndup_safe(const char *src, size_t length, const char *funcname, const char *filename, const int linenum)
{
	void *dest; /* Pointer to new string */
	size_t len; /* String length */

	if (!src)
	{
		log_output(log_level_warning, funcname, filename, linenum, "strndup() of NULL string requested");
		return NULL;
	}

	if (!length)
	{
		log_output(log_level_warning, funcname, filename, linenum, "strdup() of zero-length string requested");
		return NULL;
	}

	len = strnlen(src, length - 1) + 1;

	if (!(dest = malloc(len * sizeof(char))))
	{
		log_output(log_level_normal, funcname, filename, linenum, "strdup of %lu bytes failed", (unsigned long) len);
		perror("strdup");
		exit(EXIT_FAILURE);
	}

	len--;

	memcpy(dest, src, len * sizeof(char));

	((char *) dest)[len] = '\0';

	return dest;
}


void e_free_safe(void *ptr, const char *funcname, const char *filename, const int linenum, const bool m_warn)
{

	if (ptr)
		free(ptr);
	else
		if (m_warn) log_output(log_level_warning, funcname, filename, linenum, "Unable to free NULL pointer");

	return;
}

void *e_memset(void *m_dest, register int_fast8_t m_char, register size_t m_len)
{
	register intptr_t temp_len;
	register unsigned int temp_word;
	register uint8_t *dest = (uint8_t*)m_dest;

	if (m_len < 3 * sizeof(unsigned int))
	{
		while (m_len--) *dest++ = (uint8_t)m_char;

		return m_dest;
	}

	if ((temp_word = (uint8_t)m_char))
	{
		temp_word |= (temp_word << 24);

#		if UINT_MAX > 0xffffffff		/** 64-bit unsigned ints */
		temp_word |= (temp_word << 32);
#		endif
	}

	/**
	 * Next, we ensure our destination is word-aligned by padding the front
	 */
	if ((temp_len = (intptr_t)dest & (sizeof(unsigned int) - 1)))
	{
		temp_len = (sizeof(unsigned int) - temp_len);
		m_len -= temp_len;
		while(temp_len--) *dest++ = (uint8_t)m_char;
	}

	/**
	 * Find the remaining words.  We are at least 2 words long, so divide by 2. (16-bit)  If the word
	 * size is 4 or large, divide by 2 again (32-bit), etc...
	 */
	temp_len = m_len >> 2;

#	if UINT_MAX > 0xFFFFFFFF
	temp_len >>= 1;
#	endif


	/**
	 * Perform an aligned set of the number of words
	 */
	while (temp_len--)
	{
		*(unsigned int *)dest = temp_word;
		dest += sizeof(unsigned int);
	}

	/**
	 * Cleanup the remainder
	 */
	if ((temp_len = m_len & (sizeof(unsigned int) - 1)))
	{
		while (temp_len--) *dest++ = (uint8_t)m_char;
	}

	return m_dest;
}
